#include <pcl/visualization/cloud_viewer.h>
#include <iostream>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/passthrough.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/point_types.h>
#include <pcl/common/centroid.h>
#include <pcl/segmentation/extract_clusters.h>
#include<cmath>
#include <pcl/features/moment_of_inertia_estimation.h>
#include<pcl/console/time.h>
    
int 
main (int argc,char **argv)
{
    // std::string input_pcd;
    // if(argc==2)
    // {
    //   input_pcd=argv[1];
    // }
    // else
    // {
    //   input_pcd="/home/akita/snap/pcl_实习/pcl_xyz/25936.pcd";
    // }
    pcl::console::TicToc tt;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile ("/home/akita/snap/pcl_实习/pcl_xyz/45936.pcd",*cloud);//input_pcd

    pcl::visualization::PCLVisualizer viewer("3D Viewer");
    viewer.setCameraPosition(0,0,-0.5,0,-1,0);
    //定窗口的视角，前三个是视点坐标(x,y,z)，设置为(0,0,-3.0)说明当前视点是摄像头的后面3米。
    //后三个是视点的向上方向，(0,-1,0)表明以y轴负方向为正上方。 
    viewer.addCoordinateSystem(0.3);//加坐标系，设置显示的xyz轴的长度

    tt.tic();
    pcl::PassThrough<pcl::PointXYZ> pass;
    pass.setInputCloud (cloud);
    pass.setFilterFieldName ("y");
    pass.setFilterLimits (-0.13, 0.08);
    //2\3\5\6\7\9 -0.15, 0.001 //1\4 -0.15, 0.08 //8\10\15\16 -0.15, 0.05 
    //11\12\13\14 -0.15,0.01

    //1\9 -0.13, 0.02  (second)
    pass.filter (*cloud);

    // Create the filtering object//下采样，减少点数。不做这步，会有nan值，下面法线做不了。
    pcl::VoxelGrid<pcl::PointXYZ> sor;
    sor.setInputCloud (cloud);
    sor.setLeafSize (0.006f, 0.006f, 0.006f);
    sor.filter (*cloud_filtered);

    //法线 
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
    ne.setInputCloud (cloud_filtered);
    ne.setSearchMethod (tree);
    // Use all neighbors in a sphere of radius 3cm
    ne.setRadiusSearch (0.03);
    // Compute the features
    ne.compute (*cloud_normals);
    
    //区域增长法
    pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;
    reg.setMinClusterSize (400);
    reg.setMaxClusterSize (10000);
    reg.setSearchMethod (tree);
    reg.setNumberOfNeighbours (10);
    reg.setInputCloud (cloud_filtered);
    reg.setInputNormals (cloud_normals);
    reg.setSmoothnessThreshold (8.0 / 180.0 * M_PI);
    reg.setCurvatureThreshold (0.01);

    std::vector <pcl::PointIndices> clusters;
    reg.extract (clusters);

    //std::cout << "Number of clusters is equal to " << clusters.size () << std::endl;//测试要
    pcl::PointCloud <pcl::PointXYZRGB>::Ptr colored_cloud = reg.getColoredCloud ();
    if(colored_cloud==NULL)
    {
      std::cout<<"fail"<<std::endl;
      return -1;
    }

    Eigen::Vector4f centroid;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>); 
    pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;
    Eigen::Vector3f major_vector, middle_vector, minor_vector;
    Eigen::Vector3f mass_center;
    double x[clusters.size ()],angle[clusters.size ()],distance[clusters.size ()];
    int j=0,k;
    /*为了从点云索引向量中分割出每个聚类，必须迭代访问点云索引，每次创建一个新的点云数据集，并且将所有当前聚类的点写入到点云数据集中。*/ 
    //迭代访问点云索引cluster_indices，直到分割出所有聚类 
    for (std::vector<pcl::PointIndices>::const_iterator it = clusters.begin (); it != clusters.end (); ++it,j++) 
    { 
      for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit) 
          {
            cloud_cluster->points.push_back (cloud_filtered->points[*pit]); 
          }
      cloud_cluster->width = cloud_cluster->points.size (); 
      cloud_cluster->height = 1; 
      cloud_cluster->is_dense = true; 
      
      pcl::compute3DCentroid(*cloud_cluster,centroid);//中心点
      std::cout << "The XYZ coordinates of the centroid are: ("<< centroid[0] << ", " << centroid[1] << ", " 
                << centroid[2] << ").\n" << std::endl;
      x[j]=fabs(centroid[0]);
      distance[j]=sqrt(pow(centroid[0],2)+pow(centroid[1],2)+pow(centroid[2],2));//距离

      //包围盒算法
      feature_extractor.setInputCloud (cloud_cluster);
      feature_extractor.compute ();

      feature_extractor.getEigenVectors (major_vector, middle_vector, minor_vector);
      feature_extractor.getMassCenter (mass_center);

      pcl::PointXYZ z_axis (minor_vector (0) + mass_center (0), minor_vector (1) + mass_center (1), minor_vector (2) + mass_center (2));

      angle[j]=atan2(z_axis.z,z_axis.x)*180/3.1415926;//角度
      if(angle[j]>0)
      {
        angle[j]-=90;
      }
      else
      {
        angle[j]+=90;
      }
    }

  for(k=0,j=0;j<clusters.size ();j++)
  {
    if(x[k]>x[j])
    {
      k=j;
    }
  }
  std::cout<<"\n[done,"<<tt.toc()<<"ms]"<<std::endl;
  std::cout<<"\n中心："<<x[k]<<"\n距离："<<distance[k]<<"\n角度："<<angle[k]<< std::endl;

  viewer.addPointCloud(colored_cloud);
  while(!viewer.wasStopped())
  viewer.spinOnce(100);
  return 0;
}